
#include "PCH.h"
#include "Ray.h"
#include "RenderableObject.h"

#include "constants.h"

double CSphereObject::HitDistance(const CRay &Ray)
{
/*	double mindist = Infinity;
	double Bx = Ray.Direction.x*(Ray.Origin.x-Position.x);
	double By = Ray.Direction.y*(Ray.Origin.y-Position.y);
	double Bz = Ray.Direction.z*(Ray.Origin.z-Position.z);
	double B  = 2*(Bx+By+Bz);
	double Cx = Ray.Origin.x-Position.x;
	double Cy = Ray.Origin.y-Position.y;
	double Cz = Ray.Origin.z-Position.z;
	double C  = Cx*Cx+Cy*Cy+Cz*Cz-Radius*Radius;

	double discrim = (B*B-4*C);

	if (discrim==0)
	{
		if (-B>0)
			mindist = -B*0.5f;
	}
	if((discrim!=0)||(mindist<=eps))
	{
		mindist = Infinity;
		if (discrim>0) 
		{
			double sqrtdiscrim = sqrt(discrim);
			double mindista = (-B-sqrtdiscrim)*0.5f;
			if (mindista<-eps)
				mindista = (-B+sqrtdiscrim)*0.5f;
			if (mindista > eps)
				mindist = mindista;
		}
	}
	return mindist;
	*/
	
	//PRE TEST TO LIGHTEN THE COMPUTATION
	/*double tt = Position.DotProduct(Ray.Direction)-Ray.Origin.DotProduct(Ray.Direction);
	
	CVector p = Ray.Origin+Ray.Direction*tt;
	
	double aa = p.x-Position.x;
	double bb = p.y-Position.y;
	double cc = p.z-Position.z;

	if ((aa*aa+bb*bb+cc*cc)>Radius*Radius)
		return Infinity;
*/



	CVector m = Ray.Origin-Position;
	double b = m.DotProduct(Ray.Direction);
	double c = m.DotProduct(m)-Radius*Radius;
	if (c > 0.0f && b>0.0f) return Infinity;
	
	double discrim = b*b-c;
	if (discrim<0) return Infinity;
	double sqrtdiscrim = sqrt(discrim);
	double t = -b - sqrtdiscrim;
	if (t < 0)
		t = -b + sqrtdiscrim;

	return t;

}

void CSphereObject::GetNormal(const CVector &ImpactPoint,CRay &NormalRay)
{
	NormalRay.Origin = ImpactPoint;
	NormalRay.Direction = (ImpactPoint-Position);///Radius;
	NormalRay.Direction.Normalize();
	if(NegativeObject)
		NormalRay.Direction = NormalRay.Direction*-1;
}

void CSphereObject::CausticRayLister(const CVector &Source,std::vector<CRay> &RayList)
{
	for (int i=0;i<Radius;i++)
	{
		CRay ray;
		ray.Origin=Source;
		ray.Direction=Position;
		CVector RandomVector= CVector((Radius*2*rand()/RAND_MAX)-Radius,(Radius*2*rand()/RAND_MAX)-Radius,(Radius*2*rand()/RAND_MAX)-Radius);
		RandomVector.Normalize();
		RandomVector *=Radius;
		ray.Direction += RandomVector;
		ray.Direction.Normalize();
		RayList.push_back(ray);
	}
}

bool CSphereObject::OverlapBox(const CVector &boxcenter,const CVector &boxhalfsize)
{
	CVector	Bmin;
	CVector	Bmax;
	Bmin = boxcenter - boxhalfsize;
	Bmax = boxcenter + boxhalfsize;

	double  dmin;
	double  r2 = Radius*Radius;

	dmin = 0;

	if     ( Position.x < Bmin.x ) dmin += ( Position.x - Bmin.x )*( Position.x - Bmin.x );
	else if( Position.x > Bmax.x ) dmin += ( Position.x - Bmax.x )*( Position.x - Bmax.x );     

	if     ( Position.y < Bmin.y ) dmin += ( Position.y - Bmin.y )*( Position.y - Bmin.y );
	else if( Position.y > Bmax.y ) dmin += ( Position.y - Bmax.y )*( Position.y - Bmax.y );     

	if     ( Position.z < Bmin.z ) dmin += ( Position.z - Bmin.z )*( Position.z - Bmin.z );
	else if( Position.z > Bmax.z ) dmin += ( Position.z - Bmax.z )*( Position.z - Bmax.z );     

	if( dmin <= r2 ) return true;

	return false ;
}


#include <xmmintrin.h>
bool CTriangleObject::LineToTriangleHit(const CRay &Ray, CVector & Impact)
{
	double u;
	double v;
	double w;

	CVector pq = Ray.Direction;
	CVector pa = Point1 - Ray.Origin;
	CVector pb = Point2 - Ray.Origin;
	CVector pc = Point3 - Ray.Origin;

	CVector m = pq.CrossProduct(pc);
	u = pb.DotProduct(m);
	v = -pa.DotProduct(m);
	if (u*v<0) return false;
	w = pq.DotProduct(pb.CrossProduct(pa));
	if ((u*w<0)||(v*w<0)) return false;
	double denom = u+v+w;
	if(denom==0) return false;
	denom = 1.0f/denom;
	u *= denom;
	v *= denom;
	w *= denom;
	Impact = Point1*u+Point2*v+Point3*w;
	return true;
}

double	CTriangleObject::HitDistance(const CRay &Ray)
{
/*	CVector Impact;
	if(!LineToTriangleHit(Ray,Impact)) return Infinity;

	CVector OToImpact= Impact-Ray.Origin;
	if(OToImpact.DotProduct(Ray.Direction)<0) return Infinity;

	return Impact.Distance(Ray.Origin);
*/
		CVector normal, spana, spanb;

	spana = Point2 - Point1;
	spanb = Point3 - Point1;

	normal.x = spana.y * spanb.z - spana.z * spanb.y;
	normal.y = spana.z * spanb.x - spana.x * spanb.z;
	normal.z = spana.x * spanb.y - spana.y * spanb.x;
	normal.Normalize();
	double d = normal.DotProduct(Point1);

	double div = normal.DotProduct(Ray.Direction);
	//if(fabs(div) < eps) return Infinity;

	double num = d - normal.DotProduct(Ray.Origin);
	double dist = num / div;

	if(dist<eps)
	return Infinity;

	CVector TestPoint;
	TestPoint = (Ray.Direction)*dist+Ray.Origin;

	if(IsPointInTriangle(TestPoint))
	return dist;
	else
	return Infinity;

};


void CTriangleObject::GetNormal(const CVector &ImpactPoint,CRay &NormalRay)
{
	NormalRay.Origin=ImpactPoint;

	CVector normal, spana, spanb, vec;

	spana = Point2 - Point1;
	spanb = Point3 - Point1;

	NormalRay.Direction.x = spana.y * spanb.z - spana.z * spanb.y;
	NormalRay.Direction.y = spana.z * spanb.x - spana.x * spanb.z;
	NormalRay.Direction.z = spana.x * spanb.y - spana.y * spanb.x;
	NormalRay.Direction.Normalize();

	if(NegativeObject)
		NormalRay.Direction = NormalRay.Direction;

};


void CTriangleObject::CausticRayLister(const CVector &Source,std::vector<CRay> &RayList)
{
	double LA = Point1.Distance(Point2);
	double LB = Point1.Distance(Point3);
	double LC = Point2.Distance(Point3);
	//double Surf = sqrt((LA+LB+LC)*(-LA+LB+LC)*(LA-LB+LC)*(LA+LB-LC))/16.0;
	for (int i=0;i<64;i++)
	{
		CRay ray;
		ray.Origin=Source;
		double coefA = rand()*1000.0f/RAND_MAX;
		double coefB = rand()*(1000.0f-coefA)/RAND_MAX;
		double coefC = 1000.0f-coefA-coefB;
		coefA/=1000.0f;
		coefB/=1000.0f;
		coefC/=1000.0f;
		ray.Direction=CVector(Point1.x*coefA+Point2.x*coefB+Point3.x*coefC,Point1.y*coefA+Point2.y*coefB+Point3.y*coefC,Point1.z*coefA+Point2.z*coefB+Point3.z*coefC);
		ray.Direction.Normalize();
		RayList.push_back(ray);
	}
}

bool CTriangleObject::SameSide(CVector &TestPoint,CVector &RefPoint,CVector &Apoint,CVector &Bpoint)
{
	CVector BA;
	CVector TPA;
	CVector RPA;
	BA=Bpoint-Apoint;
	TPA=TestPoint-Apoint;
	RPA=RefPoint-Apoint;
	CVector cp1;	
	CVector cp2;	
	cp1.x = BA.y*TPA.z-BA.z*TPA.y;
	cp1.y = BA.z*TPA.x-BA.x*TPA.z;
	cp1.z = BA.x*TPA.y-BA.y*TPA.x;
	cp2.x = BA.y*RPA.z-BA.z*RPA.y;
	cp2.y = BA.z*RPA.x-BA.x*RPA.z;
	cp2.z = BA.x*RPA.y-BA.y*RPA.x;
	if (cp1.DotProduct(cp2) >= 0.0) 
		return true;
	else 
		return false;

}


bool CTriangleObject::IsPointInTriangle(CVector &TestPoint)
{
	return (SameSide(TestPoint,Point1,Point2,Point3)
		&& SameSide(TestPoint,Point2,Point3,Point1)
		&& SameSide(TestPoint,Point3,Point1,Point2));
};

double CTriangleObject::Surface () const
{
	CVector Side1 = Point1;
	Side1-=Point2;

	CVector Side2 = Point3;
	Side2-=Point2;

	return fabs(Side1.CrossProduct(Side2).Distance(CVector(0.0f,0.0f,0.0f))*0.5f);
}

double CTriangleObject::SolidAngleOnSphere (const CVector ProjectionPoint) const
{
	CVector A = Point1;
	CVector B = Point2;
	CVector C = Point3;
	CVector Origin;

	A-=ProjectionPoint;
	B-=ProjectionPoint;
	C-=ProjectionPoint;

	double Numerator = A.DotProduct(B.CrossProduct(C));
	double Denominator = A.Distance(Origin)*B.Distance(Origin)*C.Distance(Origin)
		+ (A.DotProduct(B)*C.Distance(Origin))
		+ (B.DotProduct(C)*A.Distance(Origin))
		+ (C.DotProduct(A)*B.Distance(Origin));
	if (Denominator==0)
	{
		if(Numerator<0)
			return - PI;
		else
			return PI;
	}
	return atan(Numerator/Denominator)*2;
}


CVector CTriangleObject::GetBarycenter() const
{
	CVector BC = Point1;
	BC+=Point2;
	BC+=Point3;
	BC/=3;
	return BC;
}



void CTriangleObject::FindMinMax(double X0,double X1,double X2,double & Min,double & Max)
{
	Min = Max = X0;
	if(X1<Min) Min=X1;
	if(X1>Max) Max=X1;
	if(X2<Min) Min=X2;
	if(X2>Max) Max=X2;
}


bool CTriangleObject::PlaneBoxOverlap(CVector Normal, CVector Vert, CVector MaxBox)
{

	CVector VMin;
	CVector VMax;
	double v;

	v=Vert.x;

	if(Normal.x>0.0)
	{
		VMin.x=-MaxBox.x - v;
		VMax.x= MaxBox.x - v;
	}
	else
	{
		VMin.x= MaxBox.x - v;
		VMax.x=-MaxBox.x - v;
	}

	v=Vert.y;

	if(Normal.y>0.0)
	{
		VMin.y=-MaxBox.y - v;
		VMax.y= MaxBox.y - v;
	}
	else
	{
		VMin.y= MaxBox.y - v;
		VMax.y=-MaxBox.y - v;
	}

	v=Vert.z;

	if(Normal.z>0.0)
	{
		VMin.z=-MaxBox.z - v;
		VMax.z= MaxBox.z - v;
	}
	else
	{
		VMin.z= MaxBox.z - v;
		VMax.z=-MaxBox.z - v;
	}

	if(Normal.DotProduct(VMin)>0.0f) return false;
	if(Normal.DotProduct(VMax)>=0.0f) return true;
	return false;
}

/*======================== X-tests ========================*/

#define AXISTEST_X01(a, b, fa, fb)			   \
	p0 = a*v0.y - b*v0.z;			       	   \
	p2 = a*v2.y - b*v2.z;			       	   \
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
	rad = fa * boxhalfsize.y + fb * boxhalfsize.z;   \
	if(min>rad || max<-rad) return 0;

#define AXISTEST_X2(a, b, fa, fb)			   \
	p0 = a*v0.y - b*v0.z;			           \
	p1 = a*v1.y - b*v1.z;			       	   \
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * boxhalfsize.y + fb * boxhalfsize.z;   \
	if(min>rad || max<-rad) return 0;

/*======================== Y-tests ========================*/

#define AXISTEST_Y02(a, b, fa, fb)			   \
	p0 = -a*v0.x + b*v0.z;		      	   \
	p2 = -a*v2.x + b*v2.z;	       	       	   \
	if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
	rad = fa * boxhalfsize.x + fb * boxhalfsize.z;   \
	if(min>rad || max<-rad) return false;

#define AXISTEST_Y1(a, b, fa, fb)			   \
	p0 = -a*v0.x + b*v0.z;		      	   \
	p1 = -a*v1.x + b*v1.z;	     	       	   \
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * boxhalfsize.x + fb * boxhalfsize.z;   \
	if(min>rad || max<-rad) return false;

/*======================== Z-tests ========================*/

#define AXISTEST_Z12(a, b, fa, fb)			   \
	p1 = a*v1.x - b*v1.y;			           \
	p2 = a*v2.x - b*v2.y;			       	   \
	if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
	rad = fa * boxhalfsize.x + fb * boxhalfsize.y;   \
	if(min>rad || max<-rad) return false;

#define AXISTEST_Z0(a, b, fa, fb)			   \
	p0 = a*v0.x - b*v0.y;				   \
	p1 = a*v1.x - b*v1.y;			           \
	if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
	rad = fa * boxhalfsize.x + fb * boxhalfsize.y;   \
	if(min>rad || max<-rad) return false;



bool CTriangleObject::triBoxOverlap(const CVector &boxcenter,const CVector &boxhalfsize,const CVector triverts[3])
{

	/*    use separating axis theorem to test overlap between triangle and box */
	/*    need to test for overlap in these directions: */
	/*    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle */
	/*       we do not even need to test these) */
	/*    2) normal of the triangle */
	/*    3) crossproduct(edge from tri, {x,y,z}-direction) */
	/*       this gives 3x3=9 more tests */

	CVector v0,v1,v2;

	//   double axis[3];

	double min,max,p0,p1,p2,rad,fex,fey,fez;

	CVector Normal,e0,e1,e2;

	/* This is the fastest branch on Sun */
	/* move everything so that the boxcenter is in (0,0,0) */

	v0 = triverts[0] - boxcenter;
	v1 = triverts[1] - boxcenter;
	v2 = triverts[2] - boxcenter;

	/* compute triangle edges */

	e0 = v1 - v0;      /* tri edge 0 */
	e1 = v2 - v1;      /* tri edge 1 */
	e2 = v0 - v2;      /* tri edge 2 */

	/* Bullet 3:  */
	/*  test the 9 tests first (this was faster) */

	fex = fabs(e0.x);
	fey = fabs(e0.y);
	fez = fabs(e0.z);

	AXISTEST_X01(e0.z, e0.y, fez, fey);
	AXISTEST_Y02(e0.z, e0.x, fez, fex);
	AXISTEST_Z12(e0.y, e0.x, fey, fex);

	fex = fabs(e1.x);
	fey = fabs(e1.y);
	fez = fabs(e1.z);

	AXISTEST_X01(e1.z, e1.y, fez, fey);
	AXISTEST_Y02(e1.z, e1.x, fez, fex);
	AXISTEST_Z0(e1.y, e1.x, fey, fex);

	fex = fabs(e2.x);
	fey = fabs(e2.y);
	fez = fabs(e2.z);

	AXISTEST_X2(e2.z, e2.y, fez, fey);
	AXISTEST_Y1(e2.z, e2.x, fez, fex);
	AXISTEST_Z12(e2.y, e2.x, fey, fex);

	/* Bullet 1: */
	/*  first test overlap in the {x,y,z}-directions */
	/*  find min, max of the triangle each direction, and test for overlap in */
	/*  that direction -- this is equivalent to testing a minimal AABB around */
	/*  the triangle against the AABB */

	/* test in X-direction */
	FindMinMax(v0.x,v1.x,v2.x,min,max);

	if(min>boxhalfsize.x || max<-boxhalfsize.x) return false;

	/* test in Y-direction */
	FindMinMax(v0.y,v1.y,v2.y,min,max);

	if(min>boxhalfsize.y || max<-boxhalfsize.y) return false;

	/* test in Z-direction */
	FindMinMax(v0.z,v1.z,v2.z,min,max);

	if(min>boxhalfsize.z || max<-boxhalfsize.z) return false;

	/* Bullet 2: */
	/*  test if the box intersects the plane of the triangle */
	/*  compute plane equation of triangle: normal*x+d=0 */

	Normal =e0.CrossProduct(e1);

	if(!PlaneBoxOverlap(Normal,v0,boxhalfsize)) return false;

	return true;   /* box and triangle overlaps */

}

bool CTriangleObject::OverlapBox(const CVector &boxcenter,const CVector &boxhalfsize)
{
	CVector vertices[3];
	vertices[0]=Point1;
	vertices[1]=Point2;
	vertices[2]=Point3;
	return triBoxOverlap(boxcenter,boxhalfsize,vertices);
}